#include "Integrator.h"

Integrator::Integrator(void)
{
}

Integrator::~Integrator(void)
{
}

void Integrator::InitSparseStiffness( int scale )
{
	this->scale=scale;
	sparse_M = (StiffChain**)malloc(scale*sizeof(StiffChain*));

	for (int i=0;i<scale;i++)
	{
		sparse_M[i] = new StiffChain;
	}

	ls_b = (float*)malloc(scale*3*sizeof(float));
	ls_buffer = (float*)malloc(scale*3*sizeof(float));
	ls_x = (float*)malloc(scale*3*sizeof(float));
	ls_A = (float**)malloc(scale*3*sizeof(float*));
	ls_index = (int**)malloc(scale*3*sizeof(int*));


	for (int i=0;i<scale*3;i++)
	{	
		ls_x[i] = 0;
		ls_A[i] = (float*)malloc(SPECTRUM*sizeof(float));
		ls_index[i] = (int*)malloc(SPECTRUM*sizeof(int));

		for (int j=0;j<SPECTRUM;j++)
		{
			ls_index[i][j]=-1;
		}
	}
}

void Integrator::SparseSolver()
{
	JacobiIter();
	//CheckGaussElimination();
}

void Integrator::JacobiIter()
{
	for (int loop=0;loop<1000;loop++)
	{
		for (int i=0;i<scale*3;i++)
		{
			float sum=0;
			int denorm;

			for (int j=0;j<SPECTRUM;j++)
			{
				if (ls_index[i][j]==-1)
				{
					break;
				}

				if (i==ls_index[i][j])
				{
					denorm=j;
					continue;
				}

				sum+=ls_A[i][j]*ls_x[ls_index[i][j]];
			}

			if (ls_A[i][denorm]!=0)
			{
				ls_buffer[i]=(ls_b[i]-sum)/ls_A[i][denorm];
			}

		}

		float residualMax=0;

		for (int i=0;i<scale*3;i++)
		{
			if (residualMax<abs(ls_buffer[i]-ls_x[i]))
			{
				residualMax=abs(ls_buffer[i]-ls_x[i]);
			}
			ls_x[i]=ls_buffer[i];
		}

		if (residualMax<TOLERATE)
		{
			printf("%d\n",loop);
			break;
		}
	}
}

void Integrator::CheckGaussElimination()
{
	{//Matrix Extension Construction
		for (int i=0;i<TEMPSCALE;i++)
		{
			for (int j=0;j<TEMPSCALE;j++)
			{
				GS[i][j]=check_A[i][j];
			}

			GS[i][TEMPSCALE]=check_b[i];
		}
	}

	{//Forward Substitution
		int max;
		double swap;

		for (int i=0;i<TEMPSCALE;i++)
		{
			max=i;

			for (int j=i+1;j<TEMPSCALE;j++)
			{
				if (abs(GS[j][i])>abs(GS[max][i]))
				{
					max=j;
				}
			}

			for (int j=0;j<=TEMPSCALE;j++)
			{
				swap=GS[max][j];
				GS[max][j]=GS[i][j];
				GS[i][j]=swap;
			}

			if (abs(GS[i][i]) < 1e-5)
			{
				printf("Singular\n");
			}

			for (int j=TEMPSCALE;j>=i;j--)
			{
				for (int k=i+1;k<TEMPSCALE;k++)
				{
					GS[k][j]-=GS[k][i]/GS[i][i]*GS[i][j];
				}
			}
		}
	}

	{//Reverse Elimination
		for (int i=TEMPSCALE-1;i>=0;i--)
		{
			GS[i][TEMPSCALE]=GS[i][TEMPSCALE]/GS[i][i];
			GS[i][i]=1;

			for (int j=i-1;j>=0;j--)
			{
				GS[j][TEMPSCALE]-=GS[j][i]*GS[i][TEMPSCALE];
				GS[j][i]=0;
			}
		}
	}

	{
		for (int i=0;i<TEMPSCALE;i++)
		{
			check_x[i]=GS[i][TEMPSCALE];
		}	
	}
}
Stiff::Stiff()
{

}

Stiff::~Stiff()
{

}